#include "vision_service_direct_funcs.h"
#include "vision_service_direct_types.h"

// #include <opencv2/opencv.hpp>

#include <unistd.h>
#include <fstream>
#include <iostream>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

// #include <sstream>
// #include <tf/transform_broadcaster.h>
// #include <message_filters/subscriber.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int16.h>
#include <zhz_msgs/FCVisionSwitchFrame.h>
#include <zhz_msgs/NedFrame.h>
#include <zhz_msgs/RCFrame.h>


sensor_msgs::NavSatFix GpsFrameToROSGNSS(const GpsFrame& gps_frame)
{
    sensor_msgs::NavSatFix gnss_msg;

    gnss_msg.header.stamp = ros::Time(gps_frame.timestamp / 1000000000, gps_frame.timestamp % 1000000000);
    gnss_msg.header.frame_id = "gps_frame";

    gnss_msg.latitude = gps_frame.gps_lat;
    gnss_msg.longitude = gps_frame.gps_lon;
    gnss_msg.altitude = gps_frame.gps_alt;

    gnss_msg.position_covariance[0] = gps_frame.accuracy_hor * gps_frame.accuracy_hor;
    gnss_msg.position_covariance[4] = gps_frame.accuracy_hor * gps_frame.accuracy_hor;
    gnss_msg.position_covariance[8] = gps_frame.accuracy_ver * gps_frame.accuracy_ver;
    gnss_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;

    switch (gps_frame.GNSS_fix_type) {
        case 0:
            gnss_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
            break;
        case 1:
        case 2:
            gnss_msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
            break;
        case 3:
        case 4:
            gnss_msg.status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
            break;
        case 5:
            gnss_msg.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
            break;
        default:
            gnss_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
    }

    gnss_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;

    return gnss_msg;
}

zhz_msgs::FCVisionSwitchFrame FCVisionSwitchFrameToROS(const FCVisionSwitchFrame& fc_frame) {
    zhz_msgs::FCVisionSwitchFrame ros_msg;
    ros_msg.header.stamp = ros::Time::now();
    ros_msg.header.frame_id = "gps_frame";
    // Fill in the ROS message fields from the FCVisionSwitchFrame struct
    ros_msg.enable_vio = fc_frame.enable_vio;
    ros_msg.enable_head_soa = fc_frame.enable_head_soa;
    ros_msg.enable_rear_soa = fc_frame.enable_rear_soa;
    ros_msg.enable_bottom_soa = fc_frame.enable_bottom_soa;
    ros_msg.enable_right_soa = fc_frame.enable_right_soa;
    ros_msg.enable_left_soa = fc_frame.enable_left_soa;
    ros_msg.enable_top_soa = fc_frame.enable_top_soa;
    ros_msg.enable_prior_soa = fc_frame.enable_prior_soa;
    ros_msg.enable_secure_landing = fc_frame.enable_secure_landing;
    ros_msg.enable_precise_landing = fc_frame.enable_precise_landing;
    ros_msg.enable_mapping_planning = fc_frame.enable_mapping_planning;

    return ros_msg;
}

zhz_msgs::NedFrame NedFrameToROS(const NedFrame& ned_frame) {
    zhz_msgs::NedFrame ros_msg;
    ros_msg.coord.resize(3);
    ros_msg.speed.resize(3);
    ros_msg.header.stamp = ros::Time(ned_frame.timestamp / 1000000000, ned_frame.timestamp % 1000000000);
    ros_msg.header.frame_id = "ned_frame";
    // Fill in the ROS message fields from the NedFrame struct
    ros_msg.coord[0] = ned_frame.coord[0];
    ros_msg.coord[1] = ned_frame.coord[1];
    ros_msg.coord[2] = ned_frame.coord[2];
    ros_msg.speed[0] = ned_frame.speed[0];
    ros_msg.speed[1] = ned_frame.speed[1];
    ros_msg.speed[2] = ned_frame.speed[2];
    ros_msg.valid = ned_frame.valid;

    // Add more fields as needed

    return ros_msg;
}

zhz_msgs::RCFrame RCFrameToROS(const RCFrame& rc_frame) {
    zhz_msgs::RCFrame ros_msg;
    ros_msg.header.stamp = ros::Time(rc_frame.timestamp / 1000000000, rc_frame.timestamp % 1000000000);
    ros_msg.header.frame_id = "rc_frame";
    // Fill in the ROS message fields from the RCFrame struct
    ros_msg.button = rc_frame.button;
    ros_msg.mode = rc_frame.mode;
    ros_msg.gimbal_pitch = rc_frame.gimbal_pitch;
    ros_msg.roll = rc_frame.roll;
    ros_msg.pitch = rc_frame.pitch;
    ros_msg.yaw = rc_frame.yaw;
    ros_msg.thrust = rc_frame.thrust;

    // Add more fields as needed
    return ros_msg;
}


int main(int argc, char** argv) {
  ros::init(argc, argv, "msg_convert_gps");
  ros::NodeHandle nh("~");

  std::string gps_topic;
  ros::param::get("~gps_topic", gps_topic);
  ros::Publisher gnss_pub = nh.advertise<sensor_msgs::NavSatFix>(gps_topic, 1, true);
  ros::Publisher FCVisionSwitchFrame_pub = nh.advertise<zhz_msgs::FCVisionSwitchFrame>("/zhz/driver/FCVisionSwitchFrame", 1, true);
  ros::Publisher NedFrame_pub = nh.advertise<zhz_msgs::NedFrame>("/zhz/driver/NedFrame", 1, true);
  ros::Publisher RCFrame_pub = nh.advertise<zhz_msgs::RCFrame>("/zhz/driver/RCFrame", 1, true);
  ros::Rate rate(5);
  int frame_id = 0;
  int ret = 0;
  // 1. open msg receiver
  ret = vision_create_device_stream();  // 0: success; -1: failed
  while(ros::ok()) {
    if (ret) {
        printf("vision_create_device_stream failed.\n");
        rate.sleep();
        continue;
    }
    GpsFrame gps_data;
    uint64_t time_now = vision_get_system_time(TimeSystem::TIME_FCS);
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_GPS, time_now, 100'000'000ULL, &gps_data);
    sensor_msgs::NavSatFix gnss_msg = GpsFrameToROSGNSS(gps_data);
    gnss_msg.header.seq = frame_id;
    gnss_pub.publish(gnss_msg);

    NedFrame ned_data;
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_NED, time_now, 50'000'000ULL, &ned_data);
    zhz_msgs::NedFrame Ned_Frame_msg = NedFrameToROS(ned_data);
    Ned_Frame_msg.header.seq = frame_id;
    NedFrame_pub.publish(Ned_Frame_msg);

    RCFrame rc_data;
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_RC, time_now, 50'000'000ULL, &ned_data);
    zhz_msgs::RCFrame RC_Frame_msg = RCFrameToROS(rc_data);
    RC_Frame_msg.header.seq = frame_id;
    RCFrame_pub.publish(RC_Frame_msg);

    FCVisionSwitchFrame FCVisionSwitch_Frame;
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_FC_VISIONSWITCH, time_now, 50'000'000ULL, &ned_data);
    zhz_msgs::FCVisionSwitchFrame FCVisionSwitch_Frame_msg = FCVisionSwitchFrameToROS(FCVisionSwitch_Frame);
    FCVisionSwitch_Frame_msg.header.seq = frame_id;
    FCVisionSwitchFrame_pub.publish(FCVisionSwitch_Frame_msg);
    
    frame_id++;
    ros::spinOnce();
    rate.sleep();
  }
  ros::shutdown();
  exit(0);
  return 0;
}